package org.nashua.tt151;

import edu.wpi.first.wpilibj.CANJaguar;
import edu.wpi.first.wpilibj.SimpleRobot;
import edu.wpi.first.wpilibj.can.CANTimeoutException;
import org.nashua.tt151.Controller.DualAction;

public class CANJagTest extends SimpleRobot {

    DualAction joy = new DualAction(1);
    CANJaguar jag1, jag2;

    public void init() {
        try {
            jag1 = new CANJaguar(2, CANJaguar.ControlMode.kVoltage);
            jag2 = new CANJaguar(3, CANJaguar.ControlMode.kVoltage);
        } catch (CANTimeoutException ex) {
            ex.printStackTrace();
        }

    }

    public void autonomous() {
    }

    public void operatorControl() {
        double voltage = 0;
        while (isOperatorControl() && isEnabled()) {
            if (joy.getRawButton(DualAction.Button.A)) {
                try {
                    voltage = voltage - .2;     //decreases the voltage by .2
                    jag1.setX(voltage);
                    jag2.setX(voltage);
                } catch (CANTimeoutException ex) {
                    ex.printStackTrace();
                }
            }
            if (joy.getRawButton(DualAction.Button.B)) {
                try {
                    voltage = 0;    //sets it to zero
                    jag1.setX(voltage);
                    jag2.setX(voltage);
                } catch (CANTimeoutException ex) {
                    ex.printStackTrace();
                }
            }
            if (joy.getRawButton(DualAction.Button.X)) {
                try {
                    voltage = 12;   //sets it to max voltage
                    jag1.setX(voltage);
                    jag2.setX(voltage);
                } catch (CANTimeoutException ex) {
                    ex.printStackTrace();
                }
            }
            if (joy.getRawButton(DualAction.Button.Y)) {
                try {
                    voltage = voltage + .2; //increases it by .2 volts
                    jag1.setX(voltage);
                    jag2.setX(voltage);
                } catch (CANTimeoutException ex) {
                    ex.printStackTrace();
                }
            }
        }
    }
}